{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# IKpy Quickstart #"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Requirements"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First, you need to install IKPy (see [installations instructions](https://github.com/Phylliade/ikpy)).\n",
    "You also need a URDF file.  \n",
    "By default, we use the files provided in the [resources](https://github.com/Phylliade/ikpy/tree/master/resources) folder of the IKPy repo."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Import the IKPy module : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "pycharm": {
     "is_executing": false
    }
   },
   "outputs": [],
   "source": [
    "import ikpy.chain\n",
    "import numpy as np\n",
    "import ikpy.utils.plot as plot_utils"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The basic element of IKPy is the kinematic `Chain`.\n",
    "To create a chain from an URDF file : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "is_executing": false
    }
   },
   "outputs": [],
   "source": [
    "my_chain = ikpy.chain.Chain.from_urdf_file(\"../resources/poppy_ergo.URDF\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note : as mentioned before, here we use a file in the resource folder."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Inverse kinematics"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In Inverse Kinematics, you want your kinematic chain to reach a 3D position in space."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To have a more general representation of position, IKPy works with homogeneous coordinates. It is a 4x4 matrix storing both position and orientation.\n",
    "Prepare your desired position as a 4x4 matrix. Here we only consider position, not orientation of the chain."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "pycharm": {
     "is_executing": false
    }
   },
   "outputs": [],
   "source": [
    "target_position = [ 0.1, -0.2, 0.1]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The angles of each joints are :  [ 0.          0.77375199 -1.26611195  0.32302683 -0.75260337  1.03033305\n",
      "  0.        ]\n"
     ]
    }
   ],
   "source": [
    "print(\"The angles of each joints are : \", my_chain.inverse_kinematics(target_position))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can check that the Inverse Kinematics is correct by comparing with the original position vector : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Computed position vector : [ 0.0999999  -0.1999999   0.09999988], original position vector : [0.1, -0.2, 0.1]\n"
     ]
    }
   ],
   "source": [
    "real_frame = my_chain.forward_kinematics(my_chain.inverse_kinematics(target_position))\n",
    "print(\"Computed position vector : %s, original position vector : %s\" % (real_frame[:3, 3], target_position))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Plotting\n",
    "And finally plot the result : "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "(If the code below doesn't work, comment the `%maplotlib widget` line, and uncomment the `%matplotlib inline` line)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "bfe8450a8f7c4d63a9988e0c14fe66e9",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "(-0.1, 0.1)"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Optional: support for 3D plotting in the NB\n",
    "# If there is a matplotlib error, uncomment the next line, and comment the line below it.\n",
    "# %matplotlib inline\n",
    "%matplotlib widget\n",
    "import matplotlib.pyplot as plt\n",
    "fig, ax = plot_utils.init_3d_figure()\n",
    "my_chain.plot(my_chain.inverse_kinematics(target_position), ax, target=target_position)\n",
    "plt.xlim(-0.1, 0.1)\n",
    "plt.ylim(-0.1, 0.1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You're done! Go to the [tutorials](https://github.com/Phylliade/ikpy/blob/master/tutorials/ikpy/getting_started.md) to understand the general concepts of the library."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.6"
  },
  "pycharm": {
   "stem_cell": {
    "cell_type": "raw",
    "metadata": {
     "collapsed": false
    },
    "source": []
   }
  },
  "widgets": {
   "state": {},
   "version": "1.1.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
